#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

using namespace std;
using namespace cv;

void find_feature_matches(
    const Mat &img_1, const Mat &img_2,
    vector<KeyPoint> &keypoints_1,
    vector<KeyPoint> &keypoints_2,
    vector<DMatch> &matches);

void pose_estimation_2d2d(
    const vector<KeyPoint> &keypoints_1,
    const vector<KeyPoint> &keypoints_2,
    const vector<DMatch> &matches,
    Mat &R, Mat &t);

Point2d pixel2cam(const Point2d &p, const Mat &K);

int main(int argc, char **argv)
{
    if (argc != 3)
    {
        cout << "usage: run_2d2d img1 img2" << endl;
        return 1;
    }

    // load images
    Mat img_1 = imread(argv[1], IMREAD_COLOR);
    Mat img_2 = imread(argv[2], IMREAD_COLOR);

    // find feature matches
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "total matches: " << matches.size() << endl;

    // estimate motion between two pictures
    Mat R, t;
    pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

    Mat t_x =
        (Mat_<double>(3, 3) << 0, -t.at<double>(2, 0), t.at<double>(1, 0),
         t.at<double>(2, 0), 0, -t.at<double>(0, 0),
         -t.at<double>(1, 0), t.at<double>(0, 0), 0);

    cout << "t^R = \n" << t_x * R << endl;

    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    for (DMatch m : matches)
    {
        Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);
        Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
        Mat d = y2.t() * t_x * R * y1;
        cout << "epipolar constraint = " << d << endl;
    }

    return 0;
}

Point2d pixel2cam(const Point2d &p, const Mat &K)
{
    return Point2d(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          vector<KeyPoint> &keypoints_1,
                          vector<KeyPoint> &keypoints_2,
                          vector<DMatch> &matches)
{
    // create ORB objects:
    //     - feature detector
    //     - descriptor extractor
    //     - descriptor matcher
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");

    // 1. detect keypoints of oriented FAST
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    // 2. compute BRIEF descriptors according to keypoints
    Mat descriptors_1, descriptors_2;
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    // 3. match BRIEF descriptors between two pictures
    vector<DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);

    // 4. filter the matches
    double min_dist = 10000, max_dist = 0;

    // 4.1 find the min/max distances
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        double dist = match[i].distance;
        if (dist < min_dist)
            min_dist = dist;
        if (dist > max_dist)
            max_dist = dist;
    }
    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);

    // 4.2 collect the right matches
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        if (match[i].distance <= max(2 * min_dist, 30.0))
        {
            matches.push_back(match[i]);
        }
    }
}

void pose_estimation_2d2d(const vector<KeyPoint> &keypoints_1,
                          const vector<KeyPoint> &keypoints_2,
                          const vector<DMatch> &matches,
                          Mat &R, Mat &t)
{
    // Camera intrinsics, TUM Freiburg2
    //     (fx  0 cx)   (520.9     0 325.1)
    // K = ( 0 fy cy) = (    0 521.0 249.7)
    //     ( 0  0  1)   (    0     0     1)
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

    // 1. convert the matches into 2d points
    vector<Point2f> points1;
    vector<Point2f> points2;
    for (int i = 0; i < (int)matches.size(); i++)
    {
        points1.push_back(keypoints_1[matches[i].queryIdx].pt);
        points2.push_back(keypoints_2[matches[i].trainIdx].pt);
    }

    //   T  -T  ^    -1
    // p2  K   t  R K   p1 = 0     epipolar constraints
    //   T
    // p2  F p1 = 0
    //      ^
    // E = t  R                    essential matrix
    //      -T    -1
    // F = K   E K                 fundamental matrix

    // 2. compute fundamental matrix
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat(points1, points2, FM_8POINT);
    cout << "fundamental_matrix is\n" << fundamental_matrix << endl;

    // 3. compute essential matrix
    Point2d principal_point(325.1, 249.7); // camera center(cx, cy)
    double focal_length = 521;             // camera focal(fy)
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    cout << "essential_matrix is\n " << essential_matrix << endl;

    //    likely
    // p2   =    H p1    homography matrix

    // 4. compute homography matrix
    //  it's not useful if not in a plane
    Mat homography_matrix;
    homography_matrix = findHomography(points1, points2, RANSAC, 3);
    cout << "homography_matrix is \n" << homography_matrix << endl;

    // 5. recover rotation and tranlation from essential matrix
    //   only in opencv3
    recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    cout << "R is \n" << R << endl;
    cout << "t is \n" << t << endl;
}
